###### Server in LuBanCat order to control kata-robot (G-code API) 
###### Author: ZHENG
###### Date: 2023-11-27
###### START

from wlkata_mirobot import WlkataMirobot
import re 
import socket
import threading
import json

arm = WlkataMirobot(portname = '/dev/ttyUSB0')
arm.home()

def receive_data(ip_port, new_client):
    while True:
        data = new_client.recv(1024).decode("gbk")
        # print(f"data is {data}")
        if not data:
            print(f"the data is uncreadble 2023-12 -5")
            new_client.close()
            break
        
        if data == "?":
            ### DEF JSON_DATA_STYLE 
            json_data_stytle = {  
                    "cmd": "Robot_State",  
                    "blockID": "123456789",  
                    "deviceData": {  
                        "name": "KT-LW-R6-1.0",  
                        "type": "array",  
                        "state": "Run",  
                        "angle": [0, 0, 0, 0, 0, 0, 1],  
                        "coordinate": [0, 0, 0, 0, 0, 0],  
                        "pwm": [0, 0],  
                        "mode": [0],  
                        "dataReg": [0],  
                        "robot_type": 1  
                    },  
                    "IO": {  
                        "m_DI": [0, 0, 0],  
                        "s1_DI": [0, 0, 0, 0, 0, 0, 0, 0],  
                        "s1_stepP": [0, 0],  
                        "s1_stepS": 0  
                    }  
                }
            
            new_angle = re.findall(r"[-+]?[.]?[\d]+(?:,\d\d\d)*[\.]?\d*(?:[eE][-+]?\d+)?", str(arm.status.angle)) 
            new_cartesian = re.findall(r"[-+]?[.]?[\d]+(?:,\d\d\d)*[\.]?\d*(?:[eE][-+]?\d+)?", str(arm.status.cartesian)) 
            json_data_stytle["deviceData"]["state"] = arm.status.state
            json_data_stytle["deviceData"]["coordinate"]= new_cartesian
            json_data_stytle["deviceData"]["angle"] = new_angle
            json_data_stytle["deviceData"]["pwm"] = arm.status.pump_pwm
            json_data_stytle["deviceData"]["mode"] = arm.status.motion_mode
            json_str = json.dumps(json_data_stytle)
            new_client.send(json_str.encode("gbk"))
            break
        else:

            json_data = json.loads(data)
            joint_angles = json_data["deviceData"]["angle"]
            coordinate = json_data["deviceData"]["coordinate"]
            ###accoring to G code control kata-robot
            arm.set_joint_angle(joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6], 5, False, True)
            send_flag = "ok"
            new_client.send(send_flag.encode("gbk"))
            break
            # arm.p2p_interpolation(joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5])          

if __name__== "__main__":
    tcp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    tcp_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
    tcp_server_socket.bind(("", 2438))
    tcp_server_socket.listen(128)

    while(1):
        new_client, ip_port = tcp_server_socket.accept()
        sub_thread = threading.Thread(target=receive_data, args=(ip_port, new_client))
        sub_thread.setDaemon(True)
        sub_thread.start()


###### Server in LuBanCat order to control kata-robot (G-code API) 
###### Author: ZHENG
###### Date: 2023-12-7
###### END